package bcit.nxt.mission;

//package p1;

import lejos.nxt.Motor;
import lejos.robotics.TachoMotor;
import lejos.robotics.navigation.SimpleNavigator;
import lejos.robotics.navigation.TachoPilot;


public class BaseTest3 {

    /**
     * @param args
     */
    public static void main(String[] args) {
        final float wheelDiam;
        final float trackWidth;
        final TachoMotor leftMotor;
        final TachoMotor rightMotor;
        final TachoPilot pilot;
        final SimpleNavigator navigator;
        final float distance = -1200f;
        final float distance2 = -750f;
        final int angleB = 720;
        final int angleC = -720;

        wheelDiam = 56.0f;

        trackWidth = 114.0f;

        leftMotor = Motor.B;
        rightMotor = Motor.C;

        pilot = new TachoPilot(wheelDiam, trackWidth, leftMotor, rightMotor);
        navigator = new SimpleNavigator(pilot);
        
        navigator.setPosition(0, 0, 0);
        //to ramp
        navigator.travel(distance);
        //turn at ramp
        Motor.B.rotate(angleB, true );
        Motor.C.rotate(angleC, true);
        
        while (Motor.B.isMoving() && Motor.C.isMoving()) {
            
        }
        //over ramp to bottom
        navigator.travel(distance2);
        //turn towards power base
        Motor.B.rotate(angleB, true);
        Motor.C.rotate(angleC, true);
        
        while (Motor.B.isMoving() && Motor.C.isMoving()) {
            
        }
        //get to power base
        navigator.travel(-1000f);

    }

}
